#include <arch/philips/lpc2103.h>
#include "sys.h"
 
void sys_init(void)
{
	// set CCLK (processor clock) to 60Mhz. Oscillator is 20Mhz
	PLLCON |= 0x01;				// Enabled PLL. Set bit 1
	PLLCFG &= ~0x7F;			// clear bits 6:0
	PLLCFG |= 0x22;				// set bits 6:5=01, 4:0 = 2
	APBDIV &= ~0x03;			// clear bits 1:0
	PLLFEED = 0xAA;				// PLL feed sequence
	PLLFEED = 0x55;
	while(!(PLLSTAT & (1 << 10)));	// wait for PLL lock (PLOCK bit is set if locked)
	PLLCON |= 0x03;				// Connect and enable PLL. Set bits 1:0
	PLLFEED = 0xAA;				// PLL feed sequence
	PLLFEED = 0x55;
	APBDIV = 0x00;				// set PCLK (peripheral clock) to 1/4 of CCLK

	SCS |= 0x01;				// use high speed GPIO [man23]
	
	MAMCR = 0;					// start change by turning off the MAM
	MAMTIM = 3;					// set MAM-Fetch cycle to 3 cclk as recommended for >40MHz
	MAMCR = 0x02;				// enable MAM 
	
	VICProtection = 0;			// enabled user-mode code to change VIC state
	VICIntSelect = 0;			// assign all interrupts to IRQ category [man50]
	MEMMAP = 0x01;				// use user-mode flash memory mapping
	
	FIODIR |= (1 << 15);		// initialize the on-board LED
	FIOSET |= (1 << 15);		// turn it off initially
}

void spi_init(void)
{
	S0SPCR |= 0x20; 			// Ignore this for now
	S0SPCR = 0x00;				// Clear SPI control Register
	PINSEL0 &= 0xFFFF00FF;		// Clear out PINSEL0 SPI registers
	PINSEL0 |= 0x00001500;		// Set SCK0 MISO0 and MOSI0.
	S0SPCCR = 250;				// SPI clock divider. PCLK @ 15MHz / 250 = 605KHz, MAX @ /150 for 100KHz
	S0SPCR = 0x0038;			// SPI control Register - Master mode, SCK active low.
	
	FIODIR |= (1 << CS0);
	FIODIR &= 0xF7FFFFFF;
	FIOSET |= (1 << CS0);
}

void spi_write(unsigned char spi_Data, int cs)
{

	FIOCLR |= (1 << cs);			// pull CS low
	S0SPDR = spi_Data;				// Write SPI data	
	while ( !(S0SPSR & 0x80));		// Wait for it to be complete
	FIOSET |= (1 << cs);			// set CS high
}

char spi_send(unsigned char spi_Data)
{
	S0SPDR = spi_Data;
	while ( !(S0SPSR & 0x80));
	return S0SPDR;
}

void spi_on(int cs)
{
	FIOCLR |= (1 << cs);
}

void spi_off(int cs)
{
	FIOSET |= (1 << cs);
}

void uart0_init(void)
{
	PINSEL0 |= 0x05;
	U0FCR = 0x07;			// 550 mode and FIF0's reset
	U0LCR = 0x83;			// UART 8N1, allow access to divider-latches
	U0DLL = 8;				// set baud to 115200
	U0DLM = 0;
	U0LCR = 0x03; 			// UART 8N1, forbid access to divider-latches
}

static inline void ua_outchar(char c)
{
	U0THR = c;
	while((U0LSR & 0x40) == 0);
}

void uart0_puts(const char *s)
{
	while (*s) {
		if (*s == '\n') {
			ua_outchar('\r'); // output a '\r' first
		}
		ua_outchar(*s);
		s++;
	}
}

void uart0_write(const char *d, int len)
{
	for(int i = 0; i < len; i++) {
		ua_outchar(d[i]);
	}
}

void led_pulse(int ms)
{
	FIOCLR |= 1 << 15;
	delay_ms_t0(ms);
	FIOSET |= 1 << 15;
	delay_ms_t0(ms);
}

static void led_blink_ISR() __attribute__ ((interrupt ("IRQ")));

void led_blink_ISR()
{
	FIOPIN ^= 1 << 15;				// toggle pin
	T1IR = 0xff;					// clear all interrupts by writing ones
	VICVectAddr = 0;
}

void led_blink(int ms)
{
	static int isBlinking = 0;
	
	if(ms == 0) {
		VICIntEnClr |= (1 << 5);	// disable timer1 interrupt
		VICVectAddr9 = 0;
		VICVectCntl9 = 0;
		T1TCR = 0;					// put timer1 in reset
		FIOSET |= 1 << 15;			// turn off LED
		isBlinking = 0;
		return;
	}
	
	if(isBlinking) {
		// just update the blink rate
		T1MR0 = ms;
		return;
	}
	
	// Configure timer1 to generate an interrupt at ms/2
	T1TCR = 2;						// put timer2 in reset
	T1CTCR = 0;						// select timer mode
	T1PR = 7500-1;					// set prescale so we have a resolution of .5ms
	T1MCR = (1 << 0) | (1 << 1);	// reset and interrupt on MAT1.0
	T1MR0 = ms;						// fires an interrupt at ms/2
	
	VICVectAddr9 = (unsigned long)led_blink_ISR;
	VICVectCntl9 = 5 | (1 << 5);
	VICIntEnable |= (1 << 5);		// enable interrupt for timer1 [man47]
	
	isBlinking = 1;
	
	T1TCR = 1;						// take timer1 out of reset
}

// delay for the specified number of ms using timer0
void delay_ms_t0(int ms)
{
	// use the timer to set an interrupt and wait until it fires
	T0TCR = 0x02;				// disable timer, put in reset mode.
	T0CTCR = 0x00;				// select timer mode
	T0PR = 15000 - 1; 				
	T0MR0 = ms;
	T0MCR = (1 << 2);			// when timer0 matches the match0 register, the timer will be disabled
	T0TCR = 0x01;				// take the timer out of reset mode and enable for counting.
	
	while((T0TCR & 0x01) == 1);	// wait for the timer to be disabled
}

// delay for the specified number of microseconds using timer0
void delay_us_t0(int us)
{
	// use the timer to set an interrupt and wait until it fires
	T0TCR = 0x02;				// disable timer, put in reset mode.
	T0CTCR = 0x00;				// select timer mode
	T0PR = 14; 					// prescale register to 15 for 15MHz PCLK - william
	T0MR0 = us;
	T0MCR = (1 << 2);			// when timer0 matches the match0 register, the timer will be disabled
	T0TCR = 0x01;				// take the timer out of reset mode and enable for counting.
	
	while((T0TCR & 0x01) == 1);	// wait for the timer to be disabled
}

// delay for the specified number of ms using timer1
void delay_ms_t1(int ms)
{
	// use the timer to set an interrupt and wait until it fires
	T1TCR = 0x02;				// disable timer, put in reset mode.
	T1CTCR = 0x00;				// select timer mode
	T1PR = 15000-1; 				// Changed from 60,000 to 15,000 for 1 KHz clock - william
	T1MR0 = ms;
	T1MCR = (1 << 2);			// when timer0 matches the match0 register, the timer will be disabled
	T1TCR = 0x01;				// take the timer out of reset mode and enable for counting.
	
	while((T1TCR & 0x01) == 1);	// wait for the timer to be disabled
}

// delay for the specified number of microseconds using timer1
void delay_us_t1(int us)
{
	// use the timer to set an interrupt and wait until it fires
	T1TCR = 0x02;				// disable timer, put in reset mode.
	T1CTCR = 0x00;				// select timer mode
	T1PR = 14; 					// Changed from 60 to 15, prescale. - william
	T1MR0 = us;
	T1MCR = (1 << 2);			// when timer0 matches the match0 register, the timer will be disabled
	T1TCR = 0x01;				// take the timer out of reset mode and enable for counting.
	
	while((T1TCR & 0x01) == 1);	// wait for the timer to be disabled
}


#define IRQ_MASK 0x80
#define FIQ_MASK 0x40

static inline unsigned asm_get_cpsr(void)
{
  unsigned long retval;
  asm volatile (" mrs  %0, cpsr" : "=r" (retval) : /* no inputs */  );
  return retval;
}

static inline void asm_set_cpsr(unsigned val)
{
  asm volatile (" msr  cpsr, %0" : /* no outputs */ : "r" (val)  );
}

unsigned enableIRQ(void)
{
  unsigned _cpsr;

  _cpsr = asm_get_cpsr();
  asm_set_cpsr(_cpsr & ~IRQ_MASK);
  return _cpsr;
}

unsigned disableIRQ(void)
{
  unsigned _cpsr;

  _cpsr = asm_get_cpsr();
  asm_set_cpsr(_cpsr | IRQ_MASK);
  return _cpsr;
}

unsigned restoreIRQ(unsigned oldCPSR)
{
  unsigned _cpsr;

  _cpsr = asm_get_cpsr();
  asm_set_cpsr((_cpsr & ~IRQ_MASK) | (oldCPSR & IRQ_MASK));
  return _cpsr;
}

unsigned enableFIQ(void)
{
  unsigned _cpsr;

  _cpsr = asm_get_cpsr();
  asm_set_cpsr(_cpsr & ~FIQ_MASK);
  return _cpsr;
}

unsigned disableFIQ(void)
{
  unsigned _cpsr;

  _cpsr = asm_get_cpsr();
  asm_set_cpsr(_cpsr | FIQ_MASK);
  return _cpsr;
}


void UNDEF_Routine() { }
void SWI_Routine() { }
void PAbt_Routine() { }
void DAbt_Routine() { }

// this default implementation can be overriden by defining USER_FIQ
#ifndef USER_FIQ
void FIQ_Routine() { }
#endif


